package fem2d;

import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.IMatrix;
import junit.framework.Assert;
import math2.MathUtilities;

import org.junit.Test;

import fem2.AbstractStructuralFrameStaticDemo;
import fem2.AbstractStructuralStaticDemo;
import fem2.Constraint;
import fem2.Debugger;
import fem2.Edge;
import fem2.Element;
import fem2.Force;
import fem2.Load;
import fem2.MaterialModel;
import fem2.Mesh;
import fem2.MeshPart;
import fem2.Model;
import fem2.Node;
import fem2.analysis.Analysis;
import fem2.element.CrisfieldTrusssElement;
import fem2.enu.EchoLevelType;
import fem2.enu.MassMatrixType;
import fem2.enu.State;
import fem2.material.StVenantKirchhoffMM;
import fem2.observer.DisplacementObserver;
import fem2.observer.TimeStepObserver;
import fem2.observer.XYGraphicalObserver;
import fem2.observer.XYSourceObserver;
import fem2.strategies.ArcLengthSolutionStrategyWithSphereConstraint;
import fem2.strategies.Strategy;

public class KuhlTwoBarProblem extends AbstractStructuralFrameStaticDemo {

	double H = 0.7;
	double B1 = 1;
	double B2 = 0.1;

	double E = 2e7;
	double A = 1.0;

	@Override
	public Mesh createMesh() {
		Mesh mesh = new Mesh();

		Node n1 = mesh.addNode(new Node(0.0, 0.0));
		Node n2 = mesh.addNode(new Node(B2, -H));
		Node n3 = mesh.addNode(new Node(B1 + B2, 0.0));

		mesh.addEdge(new Edge(n1, n2));
		mesh.addEdge(new Edge(n2, n3));

		return mesh;
	}

	@Override
	public Model createConditions(Model m) {
		Mesh mesh = m.getMesh();

		Constraint cxy = new Constraint(false, false);
		Constraint cx = new Constraint(false, true);
		Constraint cy = new Constraint(true, false);

		mesh.getNode(0).setConstraint(cxy);
		mesh.getNode(1).setConstraint(cy);
		mesh.getNode(2).setConstraint(cx);

		double L1 = Math.sqrt(Math.pow(H, 2) + Math.pow(B1, 2));
		double r = E * A * Math.pow(H, 3) / (3 * Math.sqrt(3) * Math.pow(L1, 3));

		Load l = new Force(mesh.getNode(2), 0.0, -r);
		m.addLoad(l);

		return m;
	}

	@Override
	public MaterialModel createMaterial(Model m) {
		double rho = 1.0;
		return new StVenantKirchhoffMM(E, 0.3, A, rho, State.ONE_DIMENSIONAL);
	}

	@Override
	public Element createElement(MeshPart mp, MaterialModel mm) {
		double preStress = 0.0;
		return new CrisfieldTrusssElement(mp, mm, preStress);
		// return new TrussElement(mp, mm);
	}

	public Model createModel() {

		Mesh mesh = createMesh();

		mesh.setNumberOfFields(mesh.getDimension());

		Model m = new Model(mesh);

		double rho = 1.0;
		double alpha = 100;
		double beta = 100;
		StVenantKirchhoffMM mm1 = new StVenantKirchhoffMM(alpha * E, 0.3, A, beta * rho,
				State.ONE_DIMENSIONAL);
		StVenantKirchhoffMM mm2 = new StVenantKirchhoffMM(E, 0.3, A, rho, State.ONE_DIMENSIONAL);

		double preStress = 0.0;
		Edge e1 = mesh.getEdge(0);
		Edge e2 = mesh.getEdge(1);
		m.addElement(new CrisfieldTrusssElement(e1, mm1, preStress));
		m.addElement(new CrisfieldTrusssElement(e2, mm2, preStress));

		m = createConditions(m);

		return m;
	}

	@Override
	public void addObservers(final Model m, Strategy s, TimeStepObserver o) {
		Node n1 = m.getMesh().getNode(1);
		Node n2 = m.getMesh().getNode(2);
		s.addObserver(new DisplacementObserver(n1, n2));

		XYSourceObserver xy = new XYSourceObserver() {

			@Override
			public void fireStarted() {
				double u1 = m.getMesh().getNode(1).getUHatLocal()[0];
				double u2 = m.getMesh().getNode(2).getUHatLocal()[1];
				double x = u1 / B2;
				double y = u2 / H;
				setData(x, y);
			}

			@Override
			public void fireTimeStepStarted() {
			}

			@Override
			public void fireIterationStarted() {
			}

			@Override
			public void fireIterationFinished() {
			}

			@Override
			public void fireTimeStepFinished() {
				double u1 = m.getMesh().getNode(1).getUHatLocal()[0];
				double u2 = m.getMesh().getNode(2).getUHatLocal()[1];
				// Debugger.watch("u1 = ", u1);
				// Debugger.watch("u2 = ", u2);
				// Debugger.watch("lambda = ", s.getCurrentTimeStep());
				// Debugger.watch("--------------");

				double x = u1 / B2;
				double y = u2 / H;
				setData(x, y);
			}

			@Override
			public void fireFinished() {
			}
		};

		s.addObserver(xy);
		s.addObserver(new XYGraphicalObserver(xy, "u1-u2 plot"));
	}

	@Test
	public void testKt() {
		Model m = createModel();
		m.Initialize();
		m.setX(0, new double[] { 0.0, 0.0 });
		IMatrix kt = new Array2DMatrix(2, 2);
		m.assembleKt(kt);

		double[][] ktRef = new double[][] { { 6.756494E07, -7.697481E06 },
				{ -7.697481E06, 5.388237E06 } };
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 2; j++) {
				Assert.assertEquals(ktRef[i][j] / 1e7, kt.get(i, j) / 1e7, MathUtilities.tol6);
			}
		}

		Debugger.watch(kt);
	}

	@Test
	public void testRi() {
		Model m = createModel();
		m.Initialize();
		m.setX(0, new double[] { 0.1, 0.1 });
		m.InitializeSolutionStep();
		double[] ri = new double[2];
		m.assembleRi(ri);

		double[] riRef = new double[] { 1.716849797759584e7, -0.017594242588328e7 };
		for (int i = 0; i < 2; i++) {
			Assert.assertEquals(riRef[i] / 1e7, ri[i] / 1e7, MathUtilities.tol6);
		}

		Debugger.watch(ri);
	}

	@Test
	public void testM() {
		Model m = createModel();
		m.Initialize();
		m.setX(0, new double[] { 0.0, 0.0 });
		IMatrix M = new Array2DMatrix(2, 2);
		m.assembleM(M, MassMatrixType.VARIATIONAL_MASS_LUMPING);

		/*
		 * TODO
		 */
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 2; j++) {
				// Assert.assertEquals(ktRef[i][j] / 1e7, m.get(i, j) / 1e7,
				// LinAlgUtilities.tol6);
			}
		}

		Debugger.watch(M);
	}

	public void run() {
		Model m = createModel();

		// Analysis an = new LinearStaticAnalysis(m);
		// an.getStrategy()
		// .setEchoLevel(EchoLevelType.OUTPUT, EchoLevelType.AUX1);
		// an.getStrategy().setSolverType(SolverType.BAITSCH_SOLVER_GENERIC);
		// an.getStrategy().setMaxNt(100);
		// addObservers(m, an.getStrategy());
		//
		// an.run();

		Analysis an = new Analysis(m);
		Strategy s = new ArcLengthSolutionStrategyWithSphereConstraint(m, 0.05, 0.0);
		s.setNumberOfTimeStep(50);
		s.setEchoLevel(EchoLevelType.OUTPUT, EchoLevelType.DEBUG);
		s.setMaxNt(50);
		an.setStrategy(s);

		addObservers(m, s, null);

		an.run();

		// postProcess(m);

		Debugger.warn("Analysis completed");
	}

	public static void main(String[] args) {
		AbstractStructuralStaticDemo demo = new KuhlTwoBarProblem();

		demo.run();
	}
}
